﻿using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;



namespace OpenPose.MyInterface
{
    //单帧单人的2d骨骼数据
    [Serializable]
    public class PoseKeyPoints
    {
        public static float NOT_ANGLE = 10000.0f;
        // pose_score>SCROE_THRO的才被认为是检测到的关节点
        public static float SCROE_THRO=0.01f;

        // 骨骼数(目前为25)
        public int numKey;
        // 骨骼帧数对应时间戳
        public DateTime coresp_time;
        
        // openpose原有的三个数据
        public float[] pose_x;
        public float[] pose_y;
        public float[] pose_score;

        // 通过坐标计算的关节转角
        public float[] angle;

        //每个关节对应的父节点.其中第八个关节和第一个关节互为为父节点,其父节点表示为-1
        private UInt16[] body25_father_index = 
            {1,//0
            8,//1
            1,//2
            2,//3
            3,//4
            1,//5
            5,//6
            6,//7
            1,//8
            8,//9
            9,//10
            10,//11
            8,//12
            12,//13
            13,//14
            0,//15
            0,//16
            15,//17
            17,//18
            14,//19
            19,//20
            14,//21
            11,//22
            22,//23
            11,//24
        };

        /// <summary>
        /// 将来自openpose for unity的数据格式转化为poseKeypoints
        /// </summary>
        /// <param name="poseKeypoints"></param>
        /// <param name="create_time"></param>
        public PoseKeyPoints(MultiArray<float> poseKeypoints, DateTime create_time)
        {

            //Debug.Log(JsonUtility.ToJson(poseKeypoints));
            numKey = poseKeypoints.GetSize(1);
            pose_x = new float[numKey];
            pose_y = new float[numKey];
            pose_score = new float[numKey];
            angle = new float[numKey];
            for (int i = 0; i < numKey; i++)
            {
                // 默认只获得第一个人的骨骼坐标
                //poseKeypoints.GetSize(2) * (i+{person}*numKey)
                int baseIndex = poseKeypoints.GetSize(2) * (i);
                pose_x[i] = poseKeypoints.Get(0, i, 0);
                pose_y[i] = poseKeypoints.Get(0, i, 1);
                pose_score[i] = poseKeypoints.Get(0, i, 2);
            }
            // 计算每个节点的旋转角
            for (int i = 0; i < numKey; i++)
            {
                // 只有当父节点为有效节点时才能计算角度
                if (pose_score[body25_father_index[i]] > 0)
                {
                    angle[i] = pos2angle(pose_x[i], pose_y[i], pose_x[body25_father_index[i]], pose_y[body25_father_index[i]]);
                }   
            }
            coresp_time = create_time;
        }

        /// <summary>
        /// 给qq用的构造函数
        /// </summary>
        /// <param name="numKey"></param>
        /// <param name="coresp_time"></param>
        /// <param name="pose_x"></param>
        /// <param name="pose_y"></param>
        /// <param name="pose_score"></param>
        public PoseKeyPoints(int numKey, DateTime coresp_time, float[] pose_x, float[] pose_y, float[] pose_score)
        {
            this.numKey = numKey;
            this.coresp_time = coresp_time;
            this.pose_x = pose_x;
            this.pose_y = pose_y;
            this.pose_score = pose_score;
            angle = new float[numKey];
            // 计算每个节点的旋转角
            for (int i = 0; i < numKey; i++)
            {
                // 只有当父节点为有效节点时才能计算角度
                if (pose_score[body25_father_index[i]] > 0)
                {
                    angle[i] = pos2angle(pose_x[i], pose_y[i], pose_x[body25_father_index[i]], pose_y[body25_father_index[i]]);
                }
            }
        }

        // 返回child节点相对father节点的旋转角;如果是完全重叠的两个点,则返回0
        private float pos2angle(float child_x, float child_y,float father_x,float father_y)
        {
            float x_dis = child_x - father_x;
            float y_dis = child_y - father_y;
            return (x_dis==0&&y_dis==0)?0:Mathf.Atan2(x_dis, y_dis);

        }

    }

    //public static float[] operator-(float[])
    //{

    //}

    /// <summary>
    /// PoseKeyPoints序列类,用于处理一段骨骼动作序列,并向外提供一些接口
    /// </summary>
    [Serializable]
    public class PoseTimeSeries
    {
        private int max_volume;
        // 
        public List<PoseKeyPoints> poseKeyPoints;

        /// <summary>
        /// 初始化一个序列
        /// </summary>
        /// <param name="max_volume"></param>
        public PoseTimeSeries(int max_volume)
        {
            this.max_volume = max_volume;
            poseKeyPoints = new List<PoseKeyPoints>(max_volume);
        }

        /// <summary>
        /// 获得一段时间内的骨骼帧
        /// </summary>
        /// <param name="start">DateTime,所返回的序列的开始时间</param>
        /// <param name="end">DateTime,所返回的序列的结束时间</param>
        /// <returns></returns>
        public List<PoseKeyPoints> poseInSeg(DateTime start, DateTime end)
        {
            List<PoseKeyPoints> res = new List<PoseKeyPoints>();
            foreach (PoseKeyPoints pk in poseKeyPoints)
            {
                if (pk.coresp_time >= start && pk.coresp_time <= end)
                {
                    res.Add(pk);
                }
                else if (pk.coresp_time > end)
                {
                    break;
                }
            }
            return res;
        }

        /// <summary>
        /// 获取从指定位置开始的、一段长度的骨骼
        /// </summary>
        /// <param name="start"></param>
        /// <param name="count"></param>
        /// <returns></returns>
        public List<PoseKeyPoints> poseInSeg(int start, int count)
        {
            return poseKeyPoints.GetRange(start, count);
        }

        /// <summary>
        /// 获取动作第一帧对应的时间戳
        /// </summary>
        /// <returns></returns>
        public DateTime getStartTime()
        {
            return poseKeyPoints[0].coresp_time;
        }

        /// <summary>
        /// 获取动作最后一帧对应的时间戳
        /// </summary>
        /// <returns></returns>
        public DateTime getEndTime()
        {
            return poseKeyPoints[-1].coresp_time;
        }

        /// <summary>
        /// 向序列中增加一帧骨骼
        /// 如果此时的序列已满,则删去一半的序列
        /// </summary>
        /// <param name="kp">一帧骨骼</param>
        public void add_poseKeyPoints(PoseKeyPoints kp)
        {
            if (max_volume < poseKeyPoints.Count)
            {
                //满载时删去1/2的帧
                poseKeyPoints.RemoveRange(0, poseKeyPoints.Count / 2);
            }
            poseKeyPoints.Add(kp);
        }


        /// <summary>
        /// 从长序列中抽取与短序列等长的一个序列
        /// </summary>
        /// <param name="short_series"></param>
        /// <param name="long_series"></param>
        /// <returns></returns>
        public static List<PoseKeyPoints> Fit_forward(List<PoseKeyPoints> short_series, List<PoseKeyPoints> long_series)
        {
            int last_index = 1;
            List<PoseKeyPoints> res = new List<PoseKeyPoints>(short_series.Count);
            foreach (var item in short_series)
            {
                // 对于pks中的每帧 寻找恰晚于它的那一帧
                for (int i = last_index; i < long_series.Count; i++)
                {
                    if (long_series[i].coresp_time>=item.coresp_time)
                    {
                        last_index = i;
                        res.Add(long_series[last_index]);
                        break;
                    }
                }
            }
            while (res.Count < short_series.Count)
            {
                res.Add(long_series[last_index]);
                Debug.LogWarning("长序列短于短序列");
            }
            return res;
        }


        /// <summary>
        /// 对脏值进行清理
        /// 检查第1:end帧PoseKeyPoints中的angle数组 计算本帧的角速度 如果超过阈值 进行线性插值处理
        /// </summary>
        /// <returns></returns>
        public void clean_data(float maxv)
        {
            List<PoseKeyPoints> res_pos = poseKeyPoints;
            for (int i = 1; i < poseKeyPoints.Count; i++)
            {
                var p = poseKeyPoints[i];
                for (int j = 0; j < p.angle.Length; j++)
                {
                    //对于每一个关节 只要角度变化速度超过阈值 则向前向后寻找正常值进行插值
                    double v = (this.poseKeyPoints[i - 1].angle[j]) / (this.poseKeyPoints[i - 1].coresp_time - poseKeyPoints[i].coresp_time).TotalMilliseconds;
                    if (v > maxv)
                    {
                        double start = 0;
                        double end = 0;
                        for (int t = i; t > 1; t--)
                        {
                            if(((poseKeyPoints[t].angle[j]) / (this.poseKeyPoints[t - 1].coresp_time - poseKeyPoints[t].coresp_time).TotalMilliseconds) < maxv)
                            {
                                start = poseKeyPoints[t - 1].angle[j];
                                break;
                            }
                            start = poseKeyPoints[0].angle[j];
                        }
                        for (int t = i; t < poseKeyPoints.Count; t++)
                        {
                            if (((poseKeyPoints[t].angle[j]) / (this.poseKeyPoints[t - 1].coresp_time - poseKeyPoints[t].coresp_time).TotalMilliseconds) < maxv)
                            {
                                end = poseKeyPoints[t - 1].angle[j];
                                break;
                            }
                            end = poseKeyPoints[poseKeyPoints.Count].angle[j];
                        }
                        res_pos[i].angle[j] = (float)(start + end) / 2;
                    }
                }                                             

            }
            this.poseKeyPoints = res_pos;
        }

    }

}
